package com.yukefms.jointpoint;

import com.yukefms.jointpoint.interfaces.Position;
import third_party.ihmc.euclid.geometry.tools.EuclidGeometryTools;
import third_party.ihmc.euclid.tools.EuclidCoreTools;
import third_party.ihmc.euclid.tuple3D.Vector3D;

/**
 * @Description A PointCalculator is implemented by Euclid Geometry <url>https://github.com/ihmcrobotics/euclid</url>
 * @Author Ping
 * @Date 2021/6/20 22:51
 **/
public class GeometryAdaptorForPointCalculator {
    /**
     * check if line segments formed by p2-p1 and p3-p1 is collinear, i.e., in the same line.
     * @param p1
     * @param p2
     * @param p3
     * @param distanceEpsilon
     * @param angleEpsilon
     * @return
     */
    public static boolean threePointsInLine(Position p1, Position p2, Position p3, double angleEpsilon, double distanceEpsilon) {
        return EuclidGeometryTools.areLine3DsCollinear(p1.getX(), p1.getY(), p1.getZ(), p2.getX()-p1.getX(), p2.getY() - p1.getY(), p2.getZ() - p1.getZ(),
                                                      p1.getX(), p1.getY(), p1.getZ(), p3.getX()-p1.getX(), p3.getY() - p1.getY(), p3.getZ() - p1.getZ(),
                                                       angleEpsilon, distanceEpsilon) ;
    }

    public static boolean threePointsInLine2D(Position p1, Position p2, Position p3, double angleEpsilon, double distanceEpsilon) {
        return EuclidGeometryTools.areLine2DsCollinear(p1.getX(), p1.getY(), p2.getX()-p1.getX(), p2.getY() - p1.getY(),
                p1.getX(), p1.getY(), p3.getX()-p1.getX(), p3.getY() - p1.getY(),
                angleEpsilon, distanceEpsilon) ;
    }

    /**
     *  feed into {@code angleFromFirstToSecondVector3D} method by two vector3D formed by p1->p2 and p1->p3
     * @param p1
     * @param p2
     * @param p3
     * @return
     */

    public static double angle(Position p1, Position p2, Position p3) {

        return EuclidGeometryTools.angleFromFirstToSecondVector3D(p2.getX() - p1.getX(), p2.getY() - p1.getY(),
                p2.getZ() - p1.getZ(),
                p3.getX() - p1.getX(), p3.getY() - p1.getY(), p3.getZ() - p1.getZ()
                );
    }

    public static double differenceOfTwoPositionsAtZ(Position p1, Position p2) {
        return p1.getZ() - p2.getZ() ;
    }
    public static double distanceOfTwoPositions(Position p1, Position p2) {
        return EuclidGeometryTools.distanceBetweenPoint3Ds(p1.getX(), p1.getY(), p1.getZ(),
                                                           p2.getX(), p2.getY(), p2.getZ());
    }


    public static double distanceFromPositionToLine(Position p1, Position lineStart, Position lineEnd) {
        return EuclidGeometryTools.distanceFromPoint3DToLineSegment3D(p1.getX(), p1.getY(),p1.getZ(),
                                                                      lineStart.getX(), lineStart.getY(), lineStart.getZ(),
                                                                        lineEnd.getX(), lineEnd.getY(), lineEnd.getZ());
    }

    public static double distanceFromPositionToPlane(Position p1, Position planePoint1, Position planePoint2, Position planePoint3) {
        //distanceFromPoint3DToPlane3D
        Vector3D normalPlane = new Vector3D() ;
        normalPlane.cross(planePoint1,planePoint2, planePoint3);
        return EuclidGeometryTools.distanceFromPoint3DToLine3D(p1.getX(), p1.getY(), p1.getZ(),
                                                                planePoint1.getX(), planePoint1.getY(), planePoint1.getZ() ,
                                                                normalPlane.getX(), normalPlane.getY(), normalPlane.getZ() );
    }


    public static boolean isTwoLineParallel(Position line1Start, Position line1End, Position line2Start, Position line2End, double angleEpsilon) {

        return EuclidGeometryTools.areVector3DsParallel(line1End.getX() - line1Start.getX(), line1End.getY() - line1Start.getY(), line1End.getZ() - line1Start.getZ()
                ,line2End.getX() - line2Start.getX(), line2End.getY() - line2Start.getY(), line2End.getZ() - line2Start.getZ()
                , angleEpsilon);
    }

    public static boolean isTwoLine2DParallel(Position line1Start, Position line1End, Position line2Start, Position line2End, double angleEpsilon) {

        return EuclidGeometryTools.areVector2DsParallel(line1End.getX() - line1Start.getX(), line1End.getY() - line1Start.getY(),
                line2End.getX() - line2Start.getX(), line2End.getY() - line2Start.getY()
                , angleEpsilon);
    }

    private static Vector3D  toVector3D(Position p) {
        return new Vector3D(p.getX(), p.getY(), p.getZ()) ;
    }


}
